<?php
namespace Helper;
use Phpml\Math\Matrix;

class KF{
    public $A;
    public $B;
    public $H;
    public $Q;
    public $R;
    public $x;
    public $P;
    private $k;
    private $numMeasured;
    
    function __construct($A, $B, $H, $Q, $R, $x, $P){
        $this->A = $A;
        $this->numState = $this->A->getRows();
        $this->B = $B;
        $this->H = $H;
        $this->Q = $Q;
        $this->R = $R;
        $this->x = $x;
        $this->P = $P;
        $this->k = null;
        $this->numMeasured = $this->H->getRows();

        $this->I = $this->IdentityMatrix($this->numState); // identity matrix
    }
    
    /**
    * 生成一个N*N维的单位矩阵
    * @date: 2017年11月15日 下午1:45:28
    * @author: ljn
    * @param: 
    * @return:
    */
    protected function IdentityMatrix($n){
        $array = array_fill(0,$n,array_fill(0,$n,0));
        for($i = 0;$i<$n;$i++){
            for($j = 0;$j<$n;$j++){
                if($i === $j)$array[$i][$j] = 1;
            }
        }
        return new Matrix($array, false);
    }
    
    /**
    * 过滤
    * @date: 2017年11月15日 下午2:06:47
    * @author: ljn
    * @param: 
    * @return:
    */
    public function Fitter($z){
        $x = $this->x;
		$P = $this->P;
		$A = $this->A;
		$B = $this->B;
		$H = $this->H;
		$Q = $this->Q;
		$R = $this->R;
		$I = $this->I;
		$k = $this->k;
//         $tempMat = (H->multiply(P)->multiply(H->transpose()))->add(R);
        $tempMat = $H->multiply($P)->multiply($H->transpose())->add($R);
        $tempMat = $tempMat->inverse();
        $k = $P->multiply($H->transpose())->multiply($tempMat);
        $this->k = $k;   
        // x = Ax + k(z - Hx)
        $x = $A->multiply($x)->add($k->multiply($z->subtract($H->multiply($x))) );
        $this->x = $x;
    
        // P = (I - kH)P
        $P =  $I->subtract($k->multiply($H))->multiply($P);
        $this->P = $P;
        return $x->toArray();
//         return [
//             'state'=>$x->toArray(),
//             'covariance'=>$P->toArray()
//         ];
    }
    /**
    * 预测
    * @date: 2017年11月15日 下午2:07:07
    * @author: ljn
    * @param: 
    * @return:
    */
    public function Predict($u){
        $x = $this->x;
        $P = $this->P;
        $A = $this->A;
        $B = $this->B;
        $Q = $this->Q;
        
        // x = Ax + Bu
        if ($B == null) {
            $x = $A->multiply($x);
        } else {
            $x = $A->multiply($x)->add($B->multiply($u));
        }
        $this->x = $x;
        
        // P = APA(T) + Q
        $P = $A->multiply($P)->multiply($A->transpose())->add($Q);
        $this->P = $P;
        return $x->toArray();
//         return [
//             "state"=> $x->toArray(),
//             "covariance" =>$P->toArray(),
//         ];
    }
    
}

